#include "js_task.h"

void js_task(void const * argument)
{
	while(1){
			if(ints==0 && fabs(x_s-target_x)>0.000001){
				x_s+=(target_x-x_s)/beilv;
			}
            if(ints==0 && fabs(y_s-target_y)>0.000001){
				y_s+=(target_y-y_s)/beilv;
			}
			if(ints==0 && fabs(a_s-target_angle)>0.000001){
				a_s+=(target_angle-a_s)/beilv;
			}
			arms_js(get_re,x_s,y_s,a_s,L0,L1,L2);
			arms_js(get_re_fut,target_x,target_y,target_angle,L0,L1,L2);
				if(get_re[3]==1){
					get_re[0]=get_re_copy[0];
					get_re[1]=get_re_copy[1];
					get_re[2]=get_re_copy[2];
				}
				else if(get_re[3]==0){
					get_re_copy[0]=get_re[0];
					get_re_copy[1]=get_re[1];
					get_re_copy[2]=get_re[2];
				}
				if(get_re_fut[3]==1){
					get_re_fut[0]=get_re_copy[0];
					get_re_fut[1]=get_re_copy[1];
					get_re_fut[2]=get_re_copy[2];
				}
        DM_target_angle=get_re[2];
        hx_data_1.motor_ang_set=get_re[0];
        DM_target_angle_b=get_re[1];
				//Duoj_Con(get_re[1],4);
				osDelay(1);
	}
}
